#include <opencv2/opencv.hpp>
#include <sys/prctl.h>
#include <time.h>
#include "opencv2/core.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
//#include "opencv2/dnn.hpp"
#include <stdio.h>
#include "dsst.h"
#include <string>
#include "Detectingmovingtarget.h"
#include "vibe.h"

#include "fdsst_track.h"
#include <thread>
#include <pthread.h>
#include <sched.h>
#include <unistd.h>
#include "ssSysComm.h"

FdsstAlgor::FdsstAlgor():
	trackFlag(1)
{

}

FdsstAlgor::~FdsstAlgor(){

}


int FdsstAlgor::Algorithm_Init(const char* CfgPath){
	return 0;
}

int FdsstAlgor::Algorithm_RunCfg(DetectRect* rect){

	if(rect == nullptr){
		return -1;
	}

	constexpr double xScale = VPSS_CHN2_IMAGE_WIDTH * 1.0 / VI_INPUT_WIDTH;
	constexpr double yScale = VPSS_CHN2_IMAGE_HEIGHT * 1.0/ VI_INPUT_HEIGHT;

	// 转换为中心点坐标使用
	int y = (rect->point[0][1] + rect->point[3][1])/2 ;
	int x = (rect->point[0][0] + rect->point[3][0])/2 ;

	int h = (rect->point[3][1] - rect->point[0][1]) * xScale;
	int w = (rect->point[3][0] - rect->point[0][0]) * yScale;

	x -= VI_INPUT_WIDTH/2;
	y += VI_INPUT_HEIGHT/2;

	x *= xScale;
	y *= yScale;

	pos[0] = y;
	pos[1] = x;

	size[0] = h;
	size[1] = w;

	
	INFO_PRT("Fdsst track input pos(x=%d,y=%d),size(w=%d,h=%d)\n",x,y,w,h);	
	return 0;
}

int FdsstAlgor::Algorithm_Reset(){

	return 0;
}

int FdsstAlgor::Algorithm_DoProcess(ss_video_frame* frame, std::vector<DetectRect>* resultIN, std::vector<DetectRect>* resultOUT){
	
	int trackstatus[5] = {};
	// int pos[2] = {search_rect.detect[0].y, search_rect.detect[0].x};
	// int size[2] = {search_rect.detect[0].h, search_rect.detect[0].w};
	
	

	{
		//TIMEUSE tt; //ava ??ms
		fdsst((volatile int *)(&trackFlag), frame->virt_addr[0], frame->width, frame->height,
			  (volatile int *)(pos), (volatile int *)(size), (volatile int *)(&trackstatus[0]));
	}

	
	int x = frame->width / 2 + trackstatus[2];
	int y = frame->height / 2 - trackstatus[1];
	int w = trackstatus[4];
	int h = trackstatus[3];
	
	x = x - w / 2;
	y = y - h / 2;

	INFO_PRT("track position x %d,y %d,w %d,h %d\n",x,y,w,h);


	

	//转换为左顶点坐标
	DetectRect rect;
	rect.point[0][0] =  x;
	rect.point[0][1] =  y;

	rect.point[1][0] =  x + w;
	rect.point[1][1] =  y;

	rect.point[2][0] =  x;
	rect.point[2][1] =  y + h;

	rect.point[3][0] =  x + w;
	rect.point[3][1] =  y + h;

	resultOUT->push_back(rect);
	return 0;
}

